
#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>

#include<ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include<opencv2/core/core.hpp>

#include"../../../include/System.h"

using namespace std;

class ImageGrabber
{
public:

// 传入深度图和彩色图数据
    ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){}

    void GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD);

    ORB_SLAM2::System* mpSLAM;
};



// ----------主函数入口------------
int main(int argc, char **argv)
{
    ros::init(argc, argv, "RGBD");
    ros::start();

    if(argc != 1)
    {
        cerr << endl << "Usage: rosrun ORB_SLAM2 RGBD path_to_vocabulary path_to_settings" << endl;        
        ros::shutdown();
        return 1;
    }    

    string voc_path = "Vocabulary/ORBvoc.txt";
    string config_path = "Examples/ROS/ORB_SLAM2/d435.yaml";
    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    
    /*:System类实例化一个slam，初始化：构造函数:System (voc路径，conf路径，传感器类型，是否窗口显示)
         System构造：
            1:判断传感器，给mSensor==RGBD赋值
            2 :检查，读取配置(d455.yaml)文件用cv::FileStorage
            3:加载词袋模型，实例化一个词袋
            4:创建关键针数据
            5:初始化图，实例化图
            6:创建可视化窗口
            7:初始化跟踪，局部建图，回环检测
                7.1 初始化跟踪时，调用Tracking构造函数（Tracking.cc里面），cv::FileStorage读取配置参数、
                        读取ORB参数后，创建ORB提取器；单目提取2倍特征点（初始化是2倍），双目，右边也要提取，RGBD只提取一倍特征点
    */
    ORB_SLAM2::System SLAM(voc_path,config_path,ORB_SLAM2::System::RGBD,true);

    // 实例化ImageGrabber类，并传入上面初始化完成的SLAM实例
    ImageGrabber igb(&SLAM);

    ros::NodeHandle nh;

    // message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/color/image_raw", 1);
    // message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "camera/depth/image_rect_raw", 1);

    message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/color/image_raw", 1);
    message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/aligned_depth_to_color/image_raw", 1);
    
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
    message_filters::Synchronizer<sync_pol> sync(sync_pol(10), rgb_sub,depth_sub);
    
    // 回调函数，调用ImageGrabber::GrabRGBD函数，最下面有解释
    sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2));

    ros::spin();

    // Stop all threads
    SLAM.Shutdown();

    // Save camera trajectory
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");

    ros::shutdown();

    return 0;
}


// 1:图像转成灰度，slam实例调用TrackRGBD函数，进行跟踪（传入彩色，深度，时间戳）
void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD)
{
    // Copy the ros image message to cv::Mat.
    cv_bridge::CvImageConstPtr cv_ptrRGB;
    try
    {
        cv_ptrRGB = cv_bridge::toCvShare(msgRGB);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv_bridge::CvImageConstPtr cv_ptrD;
    try
    {
        cv_ptrD = cv_bridge::toCvShare(msgD);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    // TrackRGBD返回Twc，可以定义Mat接受
    mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec());

}


